#include "config.h"

static const uint8_t ck_tbl[ NR_MOTOR_CKS ] = {
		TC2_CK1, TC2_CK8, TC2_CK64, TC2_CK256, TC2_CK1024,
};

static const uint8_t ocr_tbl[ NR_MOTOR_CKS ] = {
		83, 32, 32, 64, 64,
};

static uint8_t ck;

void motorSetSpeed( int speed )
{
	int setup = MOTOR_SPEED_MAX - speed;
	uint8_t i = 0;
	while( ocr_tbl[i] + setup > 255 ){
		setup -= 255 - ocr_tbl[i];
		++ i;
	}
	timer2Freq( (uint8_t) ( setup + ocr_tbl[i] ) );
	ck = ck_tbl[i];
}

static MsgBox defaultMb;
DIST_T motorMove( DIST_T dist, MsgBox *mb )
{
	if( dist > 0 ){
		SBI( MOTOR_PORT, MOTOR_DIR );
	}else{
		CBI( MOTOR_PORT, MOTOR_DIR );
	}
	if( mb == NULL )
		mb = &defaultMb;
	timer2SetNotice( ABS(dist), mb );
	timer2Start( ck );
	if( waitMsg( mb ) == timer2Comp ){
		return dist;
	} else {
		return ( ( dist > 0 ) ? timer2GetCount() : -timer2GetCount() );
	}
}

void motorInit( void )
{
	timer2Init( 0xFF );
	SBI( MOTOR_DDR, MOTOR_EN );
	SBI( MOTOR_DDR, MOTOR_DIR );
	motorSetSpeed( 0 );
}
